#pragma once
#include "CAction.h"

#define POSITION_CONTROL_ID 13
#define MAX_VEL 0.5
#define DEFAULT_KP_POS_VEL 2.50
#define DEFAULT_KD_POS_VEL 0.002
#define DEFAULT_KI_POS_VEL 0.01

using namespace std;

class CPosCtrlVer_Vel :
	public CAction
{
public:
	CPosCtrlVer_Vel(void);
	CPosCtrlVer_Vel( CSeaAmp &seaAmp ) { Init( seaAmp ); }
	~CPosCtrlVer_Vel(void);



	bool Init( CSeaAmp &seaAmp ) { return CAction::Init( seaAmp, POSITION_CONTROL_ID ); }

	void DoAct( void );

	void SetKP( double p ) { Kp = p; }
	double GetKP( void ) { return Kp; }

	void SetKI( double i ) { Ki = i; }
	double GetKI( void ) { return Ki; }

	void SetKD( double d ) { Kd = d; }
	double GetKD( void ) { return Kd; }
	
private:
	double Kp;
	double Ki;
	double Kd;

	double delta[3];

	double output;

	ofstream PositionData;

	//CQueryTimer timer;
	double timeInterval;
};

